#include "TF/TF.hpp"

TF::TF() :
    rclcpp ::Node("TF")
{
    RCLCPP_INFO(this->get_logger(), "Xu_Hua : TF生成 !!!");

    buffer_ = std::make_unique<tf2_ros::Buffer>(this->get_clock());
    listener_ = std::make_shared<tf2_ros::TransformListener>(*buffer_);

    tf_broadcaster_ = std::make_shared<tf2_ros::TransformBroadcaster>(this);
    tf_static_broadcaster_ = std::make_shared<tf2_ros::StaticTransformBroadcaster>(this);

    Timer_01_tf = create_wall_timer(100ms, std::bind(&TF::publish_tf_Timer_Callback, this));
    Timer_02_tf_listener = this->create_wall_timer(200ms, std::bind(&TF::get_Transform_Timer_Callback, this));

    this->publish_tf_static();
}

void TF::publish_tf_static()
{
    geometry_msgs::msg::TransformStamped transform;
    transform.header.stamp = this->get_clock()->now();
    transform.header.frame_id = "map";
    transform.child_frame_id = "target_point";
    transform.transform.translation.x = 0.8;
    transform.transform.translation.y = 0.9;
    transform.transform.translation.z = 0.0;
    tf2::Quaternion quat;
    quat.setRPY(0 * M_PI / 180, 0 * M_PI / 180, 0 * M_PI / 180); // 弧度制欧拉角转四元数
    transform.transform.rotation = tf2::toMsg(quat);             // 转成消息接口类型
    tf_static_broadcaster_->sendTransform(transform);
}

void TF::publish_tf_Timer_Callback()
{
    // static int num = 0;
    // RCLCPP_INFO(this->get_logger(), "Xu_Hua : 第 %d 次 : /tf 已发布 ", num++);

    geometry_msgs::msg::TransformStamped transform;
    transform.header.stamp = this->get_clock()->now();
    transform.header.frame_id = "target_point";
    transform.child_frame_id = "base_link";
    static float x = 0, y = 0.5;
    x += 0.005;
    y += 0.006;
    transform.transform.translation.x = x;
    transform.transform.translation.y = y;
    transform.transform.translation.z = 0;
    tf2::Quaternion quat;
    quat.setRPY(0 * M_PI / 180, 0 * M_PI / 180, 30 * M_PI / 180); // 弧度制欧拉角转四元数
    transform.transform.rotation = tf2::toMsg(quat);              // 转成消息接口类型
    tf_broadcaster_->sendTransform(transform);

    transform.header.stamp = this->get_clock()->now();
    transform.header.frame_id = "base_link";
    transform.child_frame_id = "base_link2";
    static float x2 = 0, y2 = -0.5;
    x2 -= 0.009;
    y2 = 0.005;
    transform.transform.translation.x = x2;
    transform.transform.translation.y = y2;
    transform.transform.translation.z = 0;
    // tf2::Quaternion quat;
    quat.setRPY(0 * M_PI / 180, 0 * M_PI / 180, 30 * M_PI / 180); // 弧度制欧拉角转四元数
    transform.transform.rotation = tf2::toMsg(quat);              // 转成消息接口类型
    tf_broadcaster_->sendTransform(transform);
}

void TF::get_Transform_Timer_Callback()
{
    static int num = 0;
    RCLCPP_INFO(this->get_logger(), "Xu_Hua : 第 %d 次 查询 tf 数据 ", num++);
    try
    {
        // 等待变换可用
        // const auto transform = buffer_->lookupTransform(
        //     "map", "base_link2", this->get_clock()->now(),
        //     rclcpp::Duration::from_seconds(1.0f));
        const auto transform = buffer_->lookupTransform(
            "map", "base_link2", this->get_clock()->now() - rclcpp::Duration::from_seconds(0.1f),
            rclcpp::Duration::from_seconds(0.1f));

        // 转换结果及打印
        const auto &translation = transform.transform.translation;
        const auto &rotation = transform.transform.rotation;

        double yaw, pitch, roll;
        tf2::getEulerYPR(rotation, yaw, pitch, roll); // 四元数转欧拉角

        // 将弧度转换为度
        yaw = yaw * 180.0 / M_PI;
        pitch = pitch * 180.0 / M_PI;
        roll = roll * 180.0 / M_PI;
        RCLCPP_INFO(get_logger(), "平移分量: (%f, %f, %f)", translation.x,
                    translation.y, translation.z);
        RCLCPP_INFO(get_logger(), "旋转分量: (%f, %f, %f)", roll, pitch, yaw);
    } catch (tf2::TransformException &ex)
    {
        RCLCPP_WARN(get_logger(), "异常: %s", ex.what()); // 处理异常
    }
}

int main(int argc, char **argv)
{
    rclcpp::init(argc, argv);
    auto node = std::make_shared<TF>();
    rclcpp::spin(node);
    rclcpp::shutdown();
    return 0;
}
